/*
 *  MatrixMath.cpp Library for MatrixMath
 *
 *  Created by Charlie Matlack on 12/18/10.
 *  Modified from code by RobH45345 on Arduino Forums, taken from unknown source.
 *  MatrixMath.cpp 
 */
#include "UAVX.h"
#include "MatrixMath.h"

#define NR_END 1


void MatrixCopy(real32* A, int16 n, int16 m, real32* B) {
	int16 i, j;
	for (i = 0; i < m; i++)
		for (j = 0; j < n; j++)
			B[n * i + j] = A[n * i + j];
}

//Matrix Multiplication Routine
// C = A*B
void MatrixMult(real32* A, real32* B, int16 m, int16 p, int16 n, real32* C) {
	// A = input matrix (m x p)
	// B = input matrix (p x n)
	// m = number of rows in A
	// p = number of columns in A = number of rows in B
	// n = number of columns in B
	// C = output matrix = A*B (m x n)
	int16 i, j, k;
	for (i = 0; i < m; i++)
		for (j = 0; j < n; j++) {
			C[n * i + j] = 0;
			for (k = 0; k < p; k++)
				C[n * i + j] = C[n * i + j] + A[p * i + k] * B[n * k + j];
		}
}

//Matrix Multiplication Routine
// C = A*B(transpose)
void MatrixMultTranspose(real32* A, real32* B, int16 m, int16 p, int16 n,
		real32* C) {
	// A = input matrix (m x p)
	// B = input matrix (n x p)
	// m = number of rows in A
	// p = number of columns in A = number of columns in B
	// n = number of rows in B
	// C = output matrix = A*B (m x n)
	int16 i, j, k;
	for (i = 0; i < m; i++)
		for (j = 0; j < n; j++) {
			C[n * i + j] = 0;
			for (k = 0; k < p; k++)
				C[n * i + j] = C[n * i + j] + A[p * i + k] * B[p * j + k];
		}
}

//Matrix Addition Routine
void MatrixAdd(real32* A, real32* B, int16 m, int16 n, real32* C) {
	// A = input matrix (m x n)
	// B = input matrix (m x n)
	// m = number of rows in A = number of rows in B
	// n = number of columns in A = number of columns in B
	// C = output matrix = A+B (m x n)
	int16 i, j;
	for (i = 0; i < m; i++)
		for (j = 0; j < n; j++)
			C[n * i + j] = A[n * i + j] + B[n * i + j];
}

//Matrix Subtraction Routine
void MatrixSubtract(real32* A, real32* B, int16 m, int16 n, real32* C) {
	// A = input matrix (m x n)
	// B = input matrix (m x n)
	// m = number of rows in A = number of rows in B
	// n = number of columns in A = number of columns in B
	// C = output matrix = A-B (m x n)
	int16 i, j;
	for (i = 0; i < m; i++)
		for (j = 0; j < n; j++)
			C[n * i + j] = A[n * i + j] - B[n * i + j];
}

//Matrix Transpose Routine
void MatrixTranspose(real32* A, int16 m, int16 n, real32* C) {
	// A = input matrix (m x n)
	// m = number of rows in A
	// n = number of columns in A
	// C = output matrix = the transpose of A (n x m)
	int16 i, j;
	for (i = 0; i < m; i++)
		for (j = 0; j < n; j++)
			C[m * j + i] = A[n * i + j];
}

//Matrix Transpose Routine
void MatrixMultScalar(real32* A, real32 s, int16 m, int16 n, real32* C) {
	// A = input matrix (m x n)
	// s = scalar value to multiply by
	// m = number of rows in A
	// n = number of columns in A
	// C = output matrix = the transpose of A (n x m)
	int16 i, j;
	for (i = 0; i < m; i++)
		for (j = 0; j < n; j++)
			C[m * j + i] = A[n * j + i] * s;
}

//Matrix Inversion Routine
// * This function inverts a matrix based on the Gauss Jordan method.
// * Specifically, it uses partial pivoting to improve numeric stability.
// * The algorithm is drawn from those presented in 
//	 NUMERICAL RECIPES: The Art of Scientific Computing.
// * The function returns 1 on success, 0 on failure.
// * NOTE: The argument is ALSO the result matrix, meaning the input matrix is REPLACED
int16 MatrixInvert(real32* A, int16 n) {
	// A = input matrix AND result matrix
	// n = number of rows = number of columns in A (n x n)
	int16 pivrow = 0; // keeps track of current pivot row
	int16 k, i, j; // k: overall index along diagonal; i: row index; j: col index
	int16 pivrows[n]; // keeps track of rows swaps to undo at end
	real32 tmp; // used for finding max value and making column swaps

	for (k = 0; k < n; k++) {
		// find pivot row, the row with biggest entry in current column
		tmp = 0;
		for (i = k; i < n; i++)
			if (fabs(A[i * n + k]) >= tmp) // 'Avoid using other functions inside abs()?'
			{
				tmp = fabs(A[i * n + k]);
				pivrow = i;
			}

		// check for singular matrix
		if (A[pivrow * n + k] == 0.0f)
			return 0; //Serial.println("Inversion failed due to singular matrix");

		// Execute pivot (row swap) if needed
		if (pivrow != k)
			// swap row k with pivrow
			for (j = 0; j < n; j++) {
				tmp = A[k * n + j];
				A[k * n + j] = A[pivrow * n + j];
				A[pivrow * n + j] = tmp;
			}

		pivrows[k] = pivrow; // record row swap (even if no swap happened)

		tmp = 1.0f / A[k * n + k]; // invert pivot element
		A[k * n + k] = 1.0f; // This element of input matrix becomes result matrix

		// Perform row reduction (divide every element by pivot)
		for (j = 0; j < n; j++)
			A[k * n + j] = A[k * n + j] * tmp;

		// Now eliminate all other entries in this column
		for (i = 0; i < n; i++)
			if (i != k) {
				tmp = A[i * n + k];
				A[i * n + k] = 0.0f; // The other place where in matrix becomes result mat
				for (j = 0; j < n; j++)
					A[i * n + j] = A[i * n + j] - A[k * n + j] * tmp;
			}
	}

	// Done, now need to undo pivot row swaps by doing column swaps in reverse order
	for (k = n - 1; k >= 0; k--)
		if (pivrows[k] != k)
			for (i = 0; i < n; i++) {
				tmp = A[i * n + k];
				A[i * n + k] = A[i * n + pivrows[k]];
				A[i * n + pivrows[k]] = tmp;
			}
	return 1;
}

void MatrixForceSymmetry(real32* A, int16 n)
//Matrix symmetry Routine
{
	// A = input matrix (m x n)
	// n = number of columns in A = number of columns in B
	int16 i, j;
	for (i = 0; i < n; i++)
		for (j = 0; j < (i - 1); j++) {
			A[n * i + j] = (A[n * i + j] + A[n * j + i]) / 2;
			A[n * j + i] = A[n * i + j];
		}
}

